#include <nuttx/config.h>
#include <stdio.h>
#include <poll.h>
#include <stdlib.h>
#include <string.h>
#include <visibility.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <common/Runnable.h>
#include <common/Thread.h>
#include <serialport/uart.h>
#include "gcslink.h"

static Runnable *runnable_ = NULL;

extern "C" __EXPORT int zorlink_main(int argc, char *argv[]);
extern "C" __EXPORT int zorlink_thread_main(int argc, char *argv[]);

static SerialPort *createSerialPort(int argc, char *argv[]) {
	const char *device_name = "/dev/ttyS1";
	int baudrate = 57600;

	for (int argi = 0; argi < argc; ++argi) {
		if (strcmp(argv[argi], "-d") == 0 && argi + 1 < argc) {
			device_name = argv[++argi];
		} else if (strcmp(argv[argi], "-b") == 0 && argi + 1 < argc) {
			baudrate = strtoul(argv[++argi], NULL, 10);
		}
	}

	return new Uart(device_name, baudrate, 50);
}

extern "C" int zorlink_thread_main(int argc, char *argv[])  {
	SerialPort *port_ = createSerialPort(argc, argv);
	runnable_ = new GcsLink(port_);
	runnable_->run_();
	delete runnable_;
	runnable_ = NULL;
	delete port_;
	port_ = NULL;
	return 0;
}

namespace {

void create(char *argv[]) {
	task_spawn_cmd("zorlink", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2048, zorlink_thread_main, (const char **)argv);
}

void destroy() {
	runnable_->setExitFlag();
}

bool isRunning() {
	return runnable_ != NULL;
}

void zorlink_usage(const char *reason) {
	if (reason)
		fprintf(stderr, "%s\n", reason);

	printf("usage: zorlink {start|stop|status} [-d <devicename>] [-b <baud rate>]\n");
}

}

/**
 * The zorlink deamon app only briefly exists to start the background job.
 * The stack size assigned in the Makefile does only apply to this management task.
 * The actual stack size should be set in the call to task_spawn().
 */
extern "C" __EXPORT
int zorlink_main(int argc, char *argv[]) {
	if (argc < 2) {
		zorlink_usage("missing command");
		return 0;
	}

	if (!strcmp(argv[1], "start")) {
		if (!isRunning())
			create(argv);
		else
			printf("\nzorlink is already running");
		return 0;
	}

	if (!strcmp(argv[1], "stop")) {
		if (isRunning())
			destroy();
		return 0;
	}

	if (!strcmp(argv[1], "status")) {
		printf("zorlink is %s\n", isRunning() ? "running" : "not started");
		return 0;
	}

	zorlink_usage("unrecognized command");
	return 1;
}


